#pragma once

#include <boost/numeric/ublas/vector.hpp>
#include <gsl/gsl_monte_plain.h>

#include "SimResult.h"
#include "filter.h"

class Environment;
class Track;

class ViacarTeam2 : public Robot {
	typedef boost::numeric::ublas::vector<double> vec;

public:

	struct my_monte_params {
		boost::numeric::ublas::matrix<double> m1;   // transformation matrix (4x4) in local coordinates for the sensor.
													// multiplying a vector by this matrix will give you the sensor's
													// coordinates relative to the car. This matrix is initialized in
													// in the constructor and stays the same accross the entire simulation
		boost::numeric::ublas::matrix<double> m2;	// transformation matrix (4x4) in world coordinates for the sensor and the car.
													// this matrix is the product of m1 and the car's transformation matrix. (env.GetCarMat())
													// multiplying a vector by this matrix will get you the sensor's coordinates in
													// world coordinates. This matrix is updated every step of the simulation
		vec n;										// normal vector (3) in world coordinates.
													// This is updated every step of the simulation
		Environment *env;							// pointer to the environment so the monte function can query for the field
	};

	struct SensorConstants {
		std::string Name;
		vec Xyz;					// relative position of sensor in xyz coordinates.
		vec Hpr;					// orientation of sensor in euler angles. sensors
									// are nominally oriented at the origin with normal vector
									// pointing down the positive z axis.
		double Size;				// diameter of ideal, square coil (cm)
		int Turns;					// number of turns in ideal, square coil
		double Gain;				// gain applied to sensed EMF before going through A/D
	};

	struct SensorData {
		double sensed;
		double sampled;
		double blocked;
		double rectified;
		double filtered;
		my_monte_params monte;
		iir_sos h2;
		double centered;			// the value of this sensor when the car is centered
	};
	
	double MaxSpeed;		// maximum speed in cm/s
	double MaxWheelAngle; 	// maximum wheel angle in radians
	double Width;			// width of wheelbase
	double Length;			// length of wheelbase
	double CenterOfMass;	// distance of center of mass from rear axle
	
	double Kp;				// PID parameters
	double Ki;
	double Kd;
	int IntegralPoints;			// number of points to compete in the field integral
								// If zero, will only use a single point
	int SensorBits;				// the number of bits in the A/D converter
	double SatThresh;			// saturation threshold value for sensors. If a sensor is below
								// this value, then the steering will saturate
	double CrossingFwdThresh;	// when the front forward sensor exceeds this value, we are
								// at a crossing
	double CrossingErrorThresh; // Compared to the error value to determine if we are at a crossing
	double LostThresh;			// If both the sensors go below this value, then we are lost
								// and the speed is set to 0

	std::vector<SensorConstants> Sensors;	// array of SensorConstants

public:

	ViacarTeam2();

	~ViacarTeam2();

	void Start();

	// Every step, we are allowed to update our speed and direction, that's it.
	void Step(double t, double dt);

	// Save current state in the simulation result
	void Save(SimResult::Data &data);

	// gets the speed of the robot in cm/s.
	// sets the speed by clamping within range
	double GetSpeed() const { return mSpeed * MaxSpeed; }

	// returns the angle of the wheels
	// sets the wheel angle by claming within max range
	double GetWheelAngle() const { return mDirection * MaxWheelAngle; }

	double GetCenterOfMass() const { return CenterOfMass; }

	double GetWidth() const { return Width; }
	
	double GetLength() const { return Length; }

	void SetEnvironment(Environment *env) { mEnv = env; mTrack = &env->GetTrack(); } 

private:

	// get the magnitude of the emf of one sensor signal
	double Sense(SensorData &d, const SensorConstants &c);

	// does all the analog signal processing between the sensor
	// output of the coil and the input of the A/D converter
	double DoSensorAnalog(double emf, const SensorConstants &c);

	void DoSensor(SensorData &d, const SensorConstants &c);

	unsigned int Sample(double v);

	void SetSpeed(double value);

	void SetWheelAngle(double value);

	void InitSensor(SensorData &d, const SensorConstants &c);
	
private:
	Environment *mEnv;
	Track *mTrack;

	double mSpeed;								// [0, 1.0]. 0 is stopped, 1.0 is full speed
	double mDirection;							// [-1.0, 1.0]. -1.0 is all the way left, 1.0 is all the way right.
	double mPreviousError;
	double mIntegral;
	double mError;
	double mFwdError;
	double mOutput;
	bool mCanUpdateDirection;
	int mSideOfTrack;
	bool mEquilibriated;
	double mFrontCenter;
	double mKp;
	int mMode;
	std::vector<SensorData> mSensors;
	std::map<std::string, int> mSensorMap;		// map of names to indices
	iir_sos mH3;

	int mLastX;					// used by GetWheelAngle to enforce the 20ms update rate

	int mWheelUpdate;

	// monte carlo integration data
	gsl_monte_plain_state *mWorkspace;
	gsl_rng *mRng;
};
